#include "GeometryBase.h"

CVertex3D CVertex3D::operator+(const CVertex3D& p)
{		
	CVertex3D q;
	q.m_data[0] = m_data[0] + p.m_data[0];
	q.m_data[1] = m_data[1] + p.m_data[1];
	q.m_data[2] = m_data[2] + p.m_data[2];
	return q;
}

const CVector3D CVertex3D::operator-(const CVertex3D& p)
{
	CVector3D v;
	v.m_data[0] = m_data[0] - p.m_data[0];
	v.m_data[1] = m_data[1] - p.m_data[1];
	v.m_data[2] = m_data[2] - p.m_data[2];
	return v;
}

CVertex3D CVertex3D::operator*(const double d)
{
	CVertex3D p;
	p.m_data[0] = m_data[0] * d;
	p.m_data[1] = m_data[1] * d;
	p.m_data[2] = m_data[2] * d;
	return p;
}

CVertex3D CVertex3D::operator/(const double d)
{
	if (d < SKE_ERROR_THRESHOLD)
		return *this;
	CVertex3D p;
	p.m_data[0] = m_data[0] / d;
	p.m_data[1] = m_data[1] / d;
	p.m_data[2] = m_data[2] / d;
	return p;
}

const bool CVertex3D::operator==(const CVertex3D& p)
{
	return abs(m_data[0] - p.m_data[0]) < SKE_ERROR_THRESHOLD
		&& abs(m_data[1] - p.m_data[1]) < SKE_ERROR_THRESHOLD
		&& abs(m_data[2] - p.m_data[2]) < SKE_ERROR_THRESHOLD;
}

const bool CVertex3D::operator!=(const CVertex3D& p)
{
	return !(*this==p);
}

	
const double CVertex3D::operator*(const CVertex3D& p)
{
	return m_data[0] * p.m_data[0] + m_data[1] * p.m_data[1] + m_data[2] * p.m_data[2];
}

void CVertex3D::SetValue(double d0, double d1, double d2)
{
	m_data[0] = d0; m_data[1] = d1; m_data[2] = d2;
}

void CVertex3D::Clip()
{
	for (int i = 0; i < 3; i++)
		if (abs(m_data[i]) < SKE_ERROR_THRESHOLD)
			m_data[i] = 0;
}

void CVertex3D::Normalize()
{
	double length = sqrt(m_data[0] * m_data[0]
	+ m_data[1] * m_data[1] + m_data[2] * m_data[2]);
	if (length > 0)
	{
		m_data[0] /= length;
		m_data[1] /= length;
		m_data[2] /= length;
	}
}

double CVertex3D::CalcLength()
{
	double length = sqrt(m_data[0] * m_data[0]
	+ m_data[1] * m_data[1] + m_data[2] * m_data[2]);
	return length;
}

const bool CVertex2D::operator==(const CVertex2D& p)
{
	return abs(m_data[0] - p.m_data[0]) < SKE_ERROR_THRESHOLD
		&& abs(m_data[1] - p.m_data[1]) < SKE_ERROR_THRESHOLD;
}

const double CVertex2D::operator*(const CVertex2D& p)
{
	return m_data[0] * p.m_data[0] + m_data[1] * p.m_data[1];
}

const CVector2D CVertex2D::operator-(const CVertex2D& p)
{
	CVector2D v;
	v.m_data[0] = m_data[0] - p.m_data[0];
	v.m_data[1] = m_data[1] - p.m_data[1];
	return v;
}

void CVertex2D::SetValue(double d0, double d1)
{
	m_data[0] = d0; m_data[1] = d1;
}

void CVertex2D::Clip()
{
	for (int i = 0; i < 2; i++)
		if (abs(m_data[i]) < SKE_ERROR_THRESHOLD)
			m_data[i] = 0;
}

void CVertex2D::Normalize()
{
	double length = sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1]);
	if (length > 0)
	{
		m_data[0] /= length;
		m_data[1] /= length;
	}
}

double CVertex2D::CalcLength()
{
	return sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1]);
}

const bool CVertex::operator==(const CVertex& v)
{

	//return m_v2D == v.m_v2D && m_v3D == v.m_v3D;
	//	for debugging
		return m_v3D == v.m_v3D;
}


void CVertex::Clip()
{
	m_v2D.Clip();
	m_v3D.Clip();
}

const bool CScaffold::operator==(const CScaffold&s)
{
	return (*m_start == *(s.m_start) && *m_end == *(s.m_end))
		|| (*m_start == *(s.m_end) && *m_end == *(s.m_start));
}

const bool CCurve::operator==(const CCurve curve)
{
	return *m_start == *curve.m_start
		&& *m_mid == *curve.m_mid
		&& *m_end == *curve.m_end;
}

const bool CCurve::operator!=(const CCurve curve)
{
	return !(*this == curve);
}

CVertex3D ComputeIntersection(
	double x1, double y1, double z1,
	double x2, double y2, double z2,
	double dx1, double dy1, double dz1,
	double dx2, double dy2, double dz2,
	SKE_LINEINTERSECT_SOLUTION& type
	)
{
	CVertex3D p = {0.0f, 0.0f, 0.0f};
	if (abs(dx1 * dy2 - dx2 * dy1) < SKE_ERROR_THRESHOLD
		&& abs(dy1 * dz2 - dy2 * dz1) < SKE_ERROR_THRESHOLD
		&& abs(dx1 *dz2 - dz1 * dx2) < SKE_ERROR_THRESHOLD)
	{
		//	they are either parallel or the same
		if (abs((x1 - x2) * dy1 - (y1 - y2) * dx1) < SKE_ERROR_THRESHOLD
			&& abs((z1 - z2) * dy1 - (y1 - y2) * dz1) < SKE_ERROR_THRESHOLD
			&& abs((x1 - x2) * dz1 - (z1 - z2) * dx1) < SKE_ERROR_THRESHOLD)
			type = INF_SOL;
		else
			type = NO_SOL;
		return p;
	}
	else
	{
		double x = x1 - x2;
		double y = y1 - y2;
		double z = z1 - z2;
		double area;
		area = (dy2 * z - y * dz2) * dx1
			- (dx2 * z - x * dz2) * dy1
			+ (dx2 * y - x * dy2) *dz1;
		if (abs(area) < SKE_ERROR_THRESHOLD)
		{
			type = ONE_SOL;
			double del = dy1 * dx2 - dx1 * dy2;
			double t1;
			if (abs(del) > SKE_ERROR_THRESHOLD)
				t1 = (1 / del) * (dx2 * (y2 - y1) - dy2 * (x2 - x1));
			else
			{
				del = dz1 * dx2 - dx1 * dz2;
				if (abs(del) > SKE_ERROR_THRESHOLD)
					t1 = (1 / del) * (dx2 * (z2 - z1) - dz2 * (x2 - x1));
				else
				{
					del = dy1 * dz2 - dz1 * dy2;
					t1 = (1 / del) * (dz2 * (y2 - y1) - dy2 * (z2 - z1));
				}
			}
			p.m_data[0] = x1 + t1 * dx1;
			p.m_data[1] = y1 + t1 * dy1;
			p.m_data[2] = z1 + t1 * dz1;
		}
		else
			type = NO_SOL;
		return p;
	}
}

CVertex2D ComputeIntersection(
	double x1, double y1,
	double x2, double y2,
	double dx1, double dy1,
	double dx2, double dy2,
	SKE_LINEINTERSECT_SOLUTION& type
	)
{
	double del = dy1 * dx2 - dx1 * dy2;
	CVertex2D p = {0.0f, 0.0f};
	if (abs(del) > SKE_ERROR_THRESHOLD)
	{
		type = ONE_SOL;
		double t1 = (1 / del) * (dx2 * (y2 - y1) - dy2 * (x2 - x1));
		p.m_data[0] = x1 + t1 * dx1;
		p.m_data[1] = y1 + t1 * dy1;
		return p;
	}
	else
	{
		del = (x2 - x1) * dy1 - (y2 - y1) * dx1;
		if (abs(del) < SKE_ERROR_THRESHOLD)
			type = INF_SOL;
		else
			type = NO_SOL;
		return p;
	}
}

CVertex3D ComputeIntersection(
	CVertex3D p1, 
	CVertex3D p2, 
	CVector3D v1, 
	CVector3D v2,
	SKE_LINEINTERSECT_SOLUTION& type)
{
	return ComputeIntersection(p1.m_data[0], p1.m_data[1], p1.m_data[2],
		p2.m_data[0], p2.m_data[1], p2.m_data[2],
		v1.m_data[0], v1.m_data[1], v1.m_data[2],
		v2.m_data[0], v2.m_data[1], v2.m_data[2], type);
}

CVertex2D ComputeIntersection(
	CVertex2D p1,
	CVertex2D p2,
	CVector2D v1,
	CVector2D v2,
	SKE_LINEINTERSECT_SOLUTION& type
	)
{
	return ComputeIntersection(p1.m_data[0], p1.m_data[1], p2.m_data[0], p2.m_data[1],
		v1.m_data[0], v1.m_data[1], v2.m_data[0], v2.m_data[1], type);
}


double CalcDistance(CVertex2D p1, CVertex2D p2)
{
	return sqrt((p1.m_data[0] - p2.m_data[0]) * (p1.m_data[0] - p2.m_data[0]) 
		+ (p1.m_data[1] - p2.m_data[1]) * (p1.m_data[1] - p2.m_data[1]));
}

double CalcDistance(CVertex3D p1, CVertex3D p2)
{
	return sqrt((p1.m_data[0] - p2.m_data[0]) * (p1.m_data[0] - p2.m_data[0])
		+ (p1.m_data[1] - p2.m_data[1]) * (p1.m_data[1] - p2.m_data[1])
		+ (p1.m_data[2] - p2.m_data[2]) * (p1.m_data[2] - p2.m_data[2]));
}

bool IsParallel(CVector2D v1, CVector2D v2)
{
	v1.Normalize();
	v2.Normalize();
	return abs(v1.m_data[0] * v2.m_data[1] - v1.m_data[1] * v2.m_data[0]) < SKE_ERROR_THRESHOLD;
}

bool IsParallel(CVector3D v1, CVector3D v2)
{
	v1.Normalize();
	v2.Normalize();
	return abs(v1.m_data[0] * v2.m_data[1] - v1.m_data[1] * v2.m_data[0]) < SKE_ERROR_THRESHOLD
		&& abs(v1.m_data[0] * v2.m_data[2] - v1.m_data[2] * v2.m_data[0]) < SKE_ERROR_THRESHOLD
		&& abs(v1.m_data[1] * v2.m_data[2] - v1.m_data[2] * v2.m_data[1]) < SKE_ERROR_THRESHOLD;
}

CVertex2D Lerp(CVertex2D p1, CVertex2D p2, double ratio)
{
	CVertex2D p = {(1 - ratio) * p1.m_data[0] + ratio * p2.m_data[0],
		(1 - ratio) * p1.m_data[1] + ratio * p2.m_data[1]};
	return p;
}

double Lerp(double x1, double y1, double x2, double y2, double x0)
{
	double y0;
	if (abs(x1 - x2) < SKE_ERROR_THRESHOLD)
		return y1;
	y0 = (x0 - x1) / (x2 - x1) * (y2 - y1) + y1;
	return y0;
}

CVector3D Cross(CVector3D v1, CVector3D v2)
{
	CVector3D v;
	v.m_data[0] = v1.m_data[1] * v2.m_data[2] - v1.m_data[2] * v2.m_data[1];
	v.m_data[1] = v1.m_data[2] * v2.m_data[0] - v1.m_data[0] * v2.m_data[2];
	v.m_data[2] = v1.m_data[0] * v2.m_data[1] - v1.m_data[1] * v2.m_data[0];
	return v;
}

float CalcAngle(CVector2D v1, CVector2D v2)
{
	float length1 = v1.CalcLength();
	float length2 = v2.CalcLength();
	if (abs(length1) < SKE_ERROR_THRESHOLD
		|| abs(length2) < SKE_ERROR_THRESHOLD)
		return 0.0f;
	else
		return acosf(v1 * v2 / (length1 * length2));
}

float CalcAngle(CVector3D v1, CVector3D v2)
{
	float length1 = v1.CalcLength();
	float length2 = v2.CalcLength();
	if (abs(length1) < SKE_ERROR_THRESHOLD
		|| abs(length2) < SKE_ERROR_THRESHOLD)
		return 0.0f;
	else
		return acosf(v1 * v2 / (length1 * length2));
}

//	projection
// http://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix

bool InverseMatrix(double m[16], double invOut[16])
{
	double inv[16], det;
	int i;

	inv[0] = m[5]  * m[10] * m[15] - 
		m[5]  * m[11] * m[14] - 
		m[9]  * m[6]  * m[15] + 
		m[9]  * m[7]  * m[14] +
		m[13] * m[6]  * m[11] - 
		m[13] * m[7]  * m[10];

	inv[4] = -m[4]  * m[10] * m[15] + 
		m[4]  * m[11] * m[14] + 
		m[8]  * m[6]  * m[15] - 
		m[8]  * m[7]  * m[14] - 
		m[12] * m[6]  * m[11] + 
		m[12] * m[7]  * m[10];

	inv[8] = m[4]  * m[9] * m[15] - 
		m[4]  * m[11] * m[13] - 
		m[8]  * m[5] * m[15] + 
		m[8]  * m[7] * m[13] + 
		m[12] * m[5] * m[11] - 
		m[12] * m[7] * m[9];

	inv[12] = -m[4]  * m[9] * m[14] + 
		m[4]  * m[10] * m[13] +
		m[8]  * m[5] * m[14] - 
		m[8]  * m[6] * m[13] - 
		m[12] * m[5] * m[10] + 
		m[12] * m[6] * m[9];

	inv[1] = -m[1]  * m[10] * m[15] + 
		m[1]  * m[11] * m[14] + 
		m[9]  * m[2] * m[15] - 
		m[9]  * m[3] * m[14] - 
		m[13] * m[2] * m[11] + 
		m[13] * m[3] * m[10];

	inv[5] = m[0]  * m[10] * m[15] - 
		m[0]  * m[11] * m[14] - 
		m[8]  * m[2] * m[15] + 
		m[8]  * m[3] * m[14] + 
		m[12] * m[2] * m[11] - 
		m[12] * m[3] * m[10];

	inv[9] = -m[0]  * m[9] * m[15] + 
		m[0]  * m[11] * m[13] + 
		m[8]  * m[1] * m[15] - 
		m[8]  * m[3] * m[13] - 
		m[12] * m[1] * m[11] + 
		m[12] * m[3] * m[9];

	inv[13] = m[0]  * m[9] * m[14] - 
		m[0]  * m[10] * m[13] - 
		m[8]  * m[1] * m[14] + 
		m[8]  * m[2] * m[13] + 
		m[12] * m[1] * m[10] - 
		m[12] * m[2] * m[9];

	inv[2] = m[1]  * m[6] * m[15] - 
		m[1]  * m[7] * m[14] - 
		m[5]  * m[2] * m[15] + 
		m[5]  * m[3] * m[14] + 
		m[13] * m[2] * m[7] - 
		m[13] * m[3] * m[6];

	inv[6] = -m[0]  * m[6] * m[15] + 
		m[0]  * m[7] * m[14] + 
		m[4]  * m[2] * m[15] - 
		m[4]  * m[3] * m[14] - 
		m[12] * m[2] * m[7] + 
		m[12] * m[3] * m[6];

	inv[10] = m[0]  * m[5] * m[15] - 
		m[0]  * m[7] * m[13] - 
		m[4]  * m[1] * m[15] + 
		m[4]  * m[3] * m[13] + 
		m[12] * m[1] * m[7] - 
		m[12] * m[3] * m[5];

	inv[14] = -m[0]  * m[5] * m[14] + 
		m[0]  * m[6] * m[13] + 
		m[4]  * m[1] * m[14] - 
		m[4]  * m[2] * m[13] - 
		m[12] * m[1] * m[6] + 
		m[12] * m[2] * m[5];

	inv[3] = -m[1] * m[6] * m[11] + 
		m[1] * m[7] * m[10] + 
		m[5] * m[2] * m[11] - 
		m[5] * m[3] * m[10] - 
		m[9] * m[2] * m[7] + 
		m[9] * m[3] * m[6];

	inv[7] = m[0] * m[6] * m[11] - 
		m[0] * m[7] * m[10] - 
		m[4] * m[2] * m[11] + 
		m[4] * m[3] * m[10] + 
		m[8] * m[2] * m[7] - 
		m[8] * m[3] * m[6];

	inv[11] = -m[0] * m[5] * m[11] + 
		m[0] * m[7] * m[9] + 
		m[4] * m[1] * m[11] - 
		m[4] * m[3] * m[9] - 
		m[8] * m[1] * m[7] + 
		m[8] * m[3] * m[5];

	inv[15] = m[0] * m[5] * m[10] - 
		m[0] * m[6] * m[9] - 
		m[4] * m[1] * m[10] + 
		m[4] * m[2] * m[9] + 
		m[8] * m[1] * m[6] - 
		m[8] * m[2] * m[5];

	det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

	if (det == 0)
		return false;

	det = 1.0 / det;

	for (i = 0; i < 16; i++)
		invOut[i] = inv[i] * det;

	return true;
}

bool IsInLineSegment(CVertex2D p2D, CLineSegment l)
{
	CVertex2D pStart = l.m_start->m_v2D;
	CVertex2D pEnd = l.m_end->m_v2D;
	CVector2D v1 = p2D - pStart;
	CVector2D v2 = p2D - pEnd;
	return IsParallel(v1, v2) && v1 * v2 < -SKE_ERROR_THRESHOLD;
}

bool IsInLineSegment(CVertex3D p3D, CLineSegment l)
{
	CVertex3D pStart = l.m_start->m_v3D;
	CVertex3D pEnd = l.m_end->m_v3D;
	CVector3D v1 = p3D - pStart;
	CVector3D v2 = p3D - pEnd;
	return IsParallel(v1, v2) && v1 * v2 < -SKE_ERROR_THRESHOLD;
}

bool IsInScaffold(CVertex2D p2D, CScaffold s)
{
	CVertex2D pStart = s.m_start->m_v2D;
	CVertex2D pEnd = s.m_end->m_v2D;
	CVector2D v1 = p2D - pStart;
	CVector2D v2 = p2D - pEnd;
	return IsParallel(v1, v2);
}

bool IsInScaffold(CVertex3D p3D, CScaffold s)
{
	CVertex3D pStart = s.m_start->m_v3D;
	CVertex3D pEnd = s.m_end->m_v3D;
	CVector3D v1 = p3D - pStart;
	CVector3D v2 = p3D - pEnd;
	return IsParallel(v1, v2);
}